#ifndef _1DOF_MODLE_H_
#define _1DOF_MODLE_H_

#include "../include/eigen/Eigen/Dense"
#include <math.h>
#include <stdio.h>
using namespace std;
using namespace Eigen;

// 获取重力补偿力矩
float Get_Gravity_Compensation_Torque(float q, float dq, float m, float a)
{
      // 参数
      float g = 9.81; // 重力加速度
      float I = 0;    // 转动惯量（kg·m²）
      float b = -0.1; // 阻尼系数（N·m·s/rad）

      float gravity_torque = m * g * a * sin(q); // 重力补偿
      // float inertia_torque = I * ddq;            // 惯性力
      float inertia_torque = 0;      // 惯性力
      float damping_torque = b * dq; // 阻尼力

      if (q < 0.1 && q > -0.1)
            return 0;
      else
            return inertia_torque + damping_torque + gravity_torque;
}

#endif